#include "hardware_controller/serial_parser.h"

namespace base_controller{

SerialParser::SerialParser(string port, int baud):serial_port_(port),serial_baud_(baud),serial_timeout_(2000){
    cout << "[Serial Params Set] serial_port: " << serial_port_ << " serial_baud: " << serial_baud_ << endl;

    // 打开串口
    try{ 
        serial_.setPort(serial_port_); 
        serial_.setBaudrate(serial_baud_); 
        // serial_.setFlowcontrol(serial::flowcontrol_none);
        // serial_.setParity(serial::parity_none); //default is parity_none
        // serial_.setStopbits(serial::stopbits_one);
        // serial_.setBytesize(serial::eightbits);
        serial::Timeout time_out = serial::Timeout::simpleTimeout(serial_timeout_);
        serial_.setTimeout(time_out);
        serial_.open(); 
        serial_.flushInput();
    }
    catch (serial::IOException& e){
        printf("Unable to open port, error is %s.", e.what());
    }
    if(serial_.isOpen()){
        cout << "[Serial Connect] Serial Port initialized! Connected to Hardware on port " << serial_.getPort()  << " baud: " << serial_.getBaudrate() << endl;
    }

    //清空数据
    memset(&Robot_Vel, 0, sizeof(Robot_Vel));
    memset(&Receive_Data, 0, sizeof(Receive_Data)); 
    memset(&Send_Data, 0, sizeof(Send_Data));
    memset(&Imu_Data, 0, sizeof(Imu_Data));
}

SerialParser::~SerialParser(){
    SerialClose();
}


//关闭串口
void SerialParser::SerialClose(){
    if (serial_.isOpen()){
        serial_.close();
        cout << "[Serial Connect] Serial Port closed successfully!" << endl;
    }
}

/**************************************
功能: 串口通讯校验函数，数据包n有个字节，第n-1个字节为校验位，第n个字节位帧尾。第1个字节到第n-2个字节数据按位异或的结果与第n-1个字节对比，即为BCC校验
输入参数： Count_Number：数据包前几个字节加入校验   mode：对发送数据还是接收数据进行校验
***************************************/
unsigned char SerialParser::Check_Sum(unsigned char Count_Number,unsigned char mode){
    unsigned char check_sum=0,k;
    
    if(mode==0){//接收数据模式
        for(k=0;k<Count_Number;k++){
            check_sum=check_sum^Receive_Data.rx[k]; //By bit or by bit //按位异或
        }
    }
    if(mode==1){ //发送数据模式
        for(k=0;k<Count_Number;k++){
            check_sum=check_sum^Send_Data.tx[k]; //By bit or by bit //按位异或
        }
    }
    return check_sum; //Returns the bitwise XOR result //返回按位异或结果
}

short SerialParser::IMU_Trans(uint8_t Data_High,uint8_t Data_Low)
{
    short transition_16;
    transition_16 = 0;
    transition_16 |=  Data_High<<8;   
    transition_16 |=  Data_Low;
    return transition_16;     
}

float SerialParser::Odom_Trans(uint8_t Data_High,uint8_t Data_Low)
{
    float data_return;
    short transition_16;
    transition_16 = 0;
    transition_16 |=  Data_High<<8;  //Get the high 8 bits of data   //获取数据的高8位
    transition_16 |=  Data_Low;      //Get the lowest 8 bits of data //获取数据的低8位
    data_return   =  (transition_16 / 1000)+(transition_16 % 1000)*0.001; // The speed unit is changed from mm/s to m/s //速度单位从mm/s转换为m/s
    return data_return;
}

bool SerialParser::GetSensorData(){
    short transition_16=0;
    // ros::Time start_time = ros::Time::now();
    uint8_t check=0,Receive_Data_Pr[1];    //临时变量，保存下位机数据
    serial_.read(Receive_Data_Pr,sizeof(Receive_Data_Pr));
    if(Receive_Data_Pr[0] == FRAME_HEADER && !start_record){
        start_record = true;
        rec_data.push_back(Receive_Data_Pr[0]);
    }
    else if(start_record && rec_data.size() < RECEIVE_DATA_SIZE){
            rec_data.push_back(Receive_Data_Pr[0]);
    }
    if(rec_data.size() != RECEIVE_DATA_SIZE) return false;
    for(int n=0; n < RECEIVE_DATA_SIZE; n++){
        Receive_Data.rx[n] = rec_data[n];
    }
    vector<uint8_t> rec_data_swap;
    rec_data.swap(rec_data_swap);
    start_record = false;

    // ros::Time end_time = ros::Time::now();
    // double total_time = (end_time - start_time).toSec(); 
    // ROS_INFO("GetSensorData total_time: %0.5f",total_time);
    // ROS_INFO("0X%02X-0X%02X-0X%02X-0X%02X-0X%02X-0X%02X-0X%02X-0X%02X-0X%02X-0X%02X-0X%02X-0X%02X-0X%02X-0X%02X-0X%02X-0X%02X-0X%02X-0X%02X-0X%02X-0X%02X-0X%02X-0X%02X-0X%02X-0X%02X",
    //     Receive_Data.rx[0],Receive_Data.rx[1],Receive_Data.rx[2],Receive_Data.rx[3],Receive_Data.rx[4],Receive_Data.rx[5],Receive_Data.rx[6],Receive_Data.rx[7],
    //     Receive_Data.rx[8],Receive_Data.rx[9],Receive_Data.rx[10],Receive_Data.rx[11],Receive_Data.rx[12],Receive_Data.rx[13],Receive_Data.rx[14],Receive_Data.rx[15],
    //     Receive_Data.rx[16],Receive_Data.rx[17],Receive_Data.rx[18],Receive_Data.rx[19],Receive_Data.rx[20],Receive_Data.rx[21],Receive_Data.rx[22],Receive_Data.rx[23]);

    if(Receive_Data.rx[0] != FRAME_HEADER || Receive_Data.rx[23] != FRAME_TAIL) return false;  //帧头匹配0X7B 帧尾匹配0X7D
    if(Receive_Data.rx[22] != Check_Sum(22,READ_DATA_CHECK))  return false;     //BBC校验

    // ROS_INFO("0X%02X-0X%02X-0X%02X-0X%02X-0X%02X-0X%02X-0X%02X-0X%02X-0X%02X-0X%02X-0X%02X-0X%02X-0X%02X-0X%02X-0X%02X-0X%02X-0X%02X-0X%02X-0X%02X-0X%02X-0X%02X-0X%02X-0X%02X-0X%02X",
    //     Receive_Data.rx[0],Receive_Data.rx[1],Receive_Data.rx[2],Receive_Data.rx[3],Receive_Data.rx[4],Receive_Data.rx[5],Receive_Data.rx[6],Receive_Data.rx[7],
    //     Receive_Data.rx[8],Receive_Data.rx[9],Receive_Data.rx[10],Receive_Data.rx[11],Receive_Data.rx[12],Receive_Data.rx[13],Receive_Data.rx[14],Receive_Data.rx[15],
    //     Receive_Data.rx[16],Receive_Data.rx[17],Receive_Data.rx[18],Receive_Data.rx[19],Receive_Data.rx[20],Receive_Data.rx[21],Receive_Data.rx[22],Receive_Data.rx[23]);  

    Receive_Data.Frame_Header = Receive_Data.rx[0]; 
    Receive_Data.Frame_Tail = Receive_Data.rx[23]; 
    Receive_Data.Flag_Stop = Receive_Data.rx[1]; //set aside //预留位
    Robot_Vel.X = Odom_Trans(Receive_Data.rx[2],Receive_Data.rx[3]); //获取运动底盘X方向速度
    Robot_Vel.Y = Odom_Trans(Receive_Data.rx[4],Receive_Data.rx[5]); //获取运动底盘Y方向速度，Y速度仅在全向移动机器人底盘有效
    Robot_Vel.Z = Odom_Trans(Receive_Data.rx[6],Receive_Data.rx[7]); //获取运动底盘Z方向速度   
    Imu_Data.accele_x_data = IMU_Trans(Receive_Data.rx[8],Receive_Data.rx[9]);   //获取IMU的X轴加速度  
    Imu_Data.accele_y_data = IMU_Trans(Receive_Data.rx[10],Receive_Data.rx[11]); //Get the Y-axis acceleration of the IMU     //获取IMU的Y轴加速度
    Imu_Data.accele_z_data = IMU_Trans(Receive_Data.rx[12],Receive_Data.rx[13]); //获取IMU的Z轴加速度
    Imu_Data.gyros_x_data = IMU_Trans(Receive_Data.rx[14],Receive_Data.rx[15]);  //获取IMU的X轴角速度  
    Imu_Data.gyros_y_data = IMU_Trans(Receive_Data.rx[16],Receive_Data.rx[17]);  //获取IMU的Y轴角速度  
    Imu_Data.gyros_z_data = IMU_Trans(Receive_Data.rx[18],Receive_Data.rx[19]);  //获取IMU的Z轴角速度  
    //获取电池电压
    transition_16 |=  Receive_Data.rx[20]<<8;
    transition_16 |=  Receive_Data.rx[21];  
    Power_voltage = transition_16/1000+(transition_16 % 1000)*0.001; //Unit conversion millivolt(mv)->volt(v) //单位转换毫伏(mv)->伏(v)
    //cout << "Robot_Vel x: " << Robot_Vel.X << " y: " << Robot_Vel.Y << " z: " << Robot_Vel.Z << " Power_voltage: " << Power_voltage << endl;
    // end_time = ros::Time::now();
    // total_time = (end_time - start_time).toSec(); 
    // ROS_INFO("total_time: %0.3f",total_time);
    return true; 
}


void SerialParser::SetMotorSpeed(double x, double y, double z){
    short  transition;  //中间变量
    Send_Data.tx[0]=FRAME_HEADER; //帧头0X7B
    Send_Data.tx[1] = 0; //预留位
    Send_Data.tx[2] = 0; //预留位

    //机器人x轴的目标线速度
    transition=0;
    transition = x*1000; //将浮点数放大一千倍，简化传输
    Send_Data.tx[4] = transition;     //取数据的低8位
    Send_Data.tx[3] = transition>>8;  //取数据的高8位
    //机器人y轴的目标线速度
    transition=0;
    transition = y*1000;
    Send_Data.tx[6] = transition;
    Send_Data.tx[5] = transition>>8;
    //机器人z轴的目标角速度
    transition=0;
    transition = z*1000;
    Send_Data.tx[8] = transition;
    Send_Data.tx[7] = transition>>8;

    Send_Data.tx[9]=Check_Sum(9,SEND_DATA_CHECK); 
    Send_Data.tx[10]=FRAME_TAIL;    //帧尾0X7D
    try{
        serial_.write(Send_Data.tx,sizeof (Send_Data.tx)); 
    }
    catch (serial::IOException& e)   {
        printf("Unable to send data through serial port!");
    }
}


}   //base_controller